#include <stdio.h>
#include <stdlib.h>
#include "rgb2yuv.h"

#define RGB2YUV(r,g,b,y,u,v)\
	y = (77*r + 150*g + 29*b)>>8;\
	u =((128*b - 43*r -85*g)>>8) +128;\
	v =((128*r - 107*g -21*b)>>8) +128;\
	y = y < 0 ? 0 : y;\
	u = u < 0 ? 0 : u;\
	v = v < 0 ? 0 : v;\
	y = y > 255 ? 255 : y;\
	u = u > 255 ? 255 : u;\
	v = v > 255 ? 255 : v

int rgb888_to_yuv422i(unsigned char *prgb888, unsigned char *pyuv422i, int w, int h)
{
    int i,j;
    unsigned char r,g,b;
    int y,u,v;
    
    if((w <= 0) || (h <= 0))
    {
        printf("width and height of mask must be greater than 0.\n");
        exit (-1);
        
    }
    
    if (((w & 0x1) != 0) || ((h & 0x1) != 0))
    {
        printf("width and height of mask must be multiple of 2.\n");
        exit (-1);
    }

    for (i = 0; i < h; i++)
    {
        for (j = 0; j < w; j++)
        {
            r = *prgb888++;
            g = *prgb888++;
            b = *prgb888++;
            RGB2YUV(b,g,r,y,u,v);
            if (j & 0x1) 
            {
                *pyuv422i++ = v;
                *pyuv422i++ = y;
            }
            else
            {
                *pyuv422i++ = u;
                *pyuv422i++ = y;
            }
        }
    }

    return 0;
}

//u通道在前 v通道在后
int rgb888_to_yuv422p(unsigned char *prgb888, unsigned char *pyuv422p, int w, int h)
{
    int i,j;
    unsigned char r,g,b;
    int y,u,v;
    unsigned char *py,*pu,*pv;
    
    if((w <= 0) || (h <= 0))
    {
        printf("width and height of mask must be greater than 0.\n");
        
        if (prgb888)
        {
            free(prgb888);
            prgb888 = NULL;
        }
        
        if (pyuv422p)
        {
            free(pyuv422p);
            pyuv422p = NULL;
        }
        exit (-1);
        
    }
    
    if (((w & 0x1) != 0) || ((h & 0x1) != 0))
    {
        printf("width and height of mask must be multiple of 2.\n");
        
        if (prgb888)
        {
            free(prgb888);
            prgb888 = NULL;
        }
        
        if (pyuv422p)
        {
            free(pyuv422p);
            pyuv422p = NULL;
        }
        exit (-1);
    }

    py = pyuv422p;
    pu = py + w * h;
    pv = pu + w * h / 2;

    for (i = 0; i < h; i++)
    {
        for (j = 0; j < w; j++)
        {
            r = *prgb888++;
            g = *prgb888++;
            b = *prgb888++;
            RGB2YUV(b,g,r,y,u,v);
            *py++ = y;
            if (j & 0x1 )//取奇数列的值，uv为y的一半
            {
//                *pu++ = u;
//                *pv++ = v;
                *pu++ = v;
                *pv++ = u;
            }
        }
    }

    return 0;
}

//v通道在前 u通道在后
int rgb888_to_yvu422p(unsigned char *prgb888, unsigned char *pyvu422p, int w, int h)
{
    int i,j;
    unsigned char r,g,b;
    int y,v,u;
    unsigned char *py,*pv,*pu;
    
    if((w <= 0) || (h <= 0))
    {
        printf("width and height of mask must be greater than 0.\n");
        exit (-1);
        
    }
    
    if (((w & 0x1) != 0) || ((h & 0x1) != 0))
    {
        printf("width and height of mask must be multiple of 2.\n");
        exit (-1);
    }

    py = pyvu422p;
    pv = py + w * h;
    pu = pv + w * h / 2;

    for (i = 0; i < h; i++)
    {
        for (j = 0; j < w; j++)
        {
            r = *prgb888++;
            g = *prgb888++;
            b = *prgb888++;
            RGB2YUV(b,g,r,y,u,v);
            *py++ = y;
            if (j & 0x1)
            {
//                *pv++ = v;
//                *pu++ = u;
                
                *pv++ = u;
                *pu++ = v;
                
            }
        }
    }

    return 0;
}


int rgb888_to_yuv420i(unsigned char *prgb888, unsigned char *pyuv420i, int w, int h)
{
    int i,j;
    unsigned char r,g,b;
    int y,u,v;
    unsigned char *py,*puv;
    
    if((w <= 0) || (h <= 0))
    {
        printf("width and height of mask must be greater than 0.\n");
        exit (-1);
        
    }
    
    if (((w & 0x1) != 0) || ((h & 0x1) != 0))
    {
        printf("width and height of mask must be multiple of 2.\n");
        exit (-1);
    }

    py = pyuv420i;
    puv = py + w * h;

    for (i = 0; i < h; i++)
    {
        for (j = 0; j < w; j++)
        {
            r = *prgb888++;
            g = *prgb888++;
            b = *prgb888++;
            RGB2YUV(b,g,r,y,u,v);
            *py++ = y;
            if ((i & 0x1) && (j & 0x1))
            {
                *puv++ = u;
                *puv++ = v;
            }
        }
    }

    return 0;
}

int rgb888_to_yuv420p(unsigned char *prgb888, unsigned char *pyuv420p, int w, int h)
{
    int i,j;
    unsigned char r,g,b;
    int y,u,v;
    unsigned char *py,*pu,*pv;
    
    if((w <= 0) || (h <= 0))
    {
        printf("width and height of mask must be greater than 0.\n");
        exit (-1);
        
    }
    
    if (((w & 0x1) != 0) || ((h & 0x1) != 0))
    {
        printf("width and height of mask must be multiple of 2.\n");
        exit (-1);
    }

    py = pyuv420p;
    pu = py + w * h;
    pv = pu + w * h / 4;

    for (i = 0; i < h; i++)
    {
        for (j = 0; j < w; j++)
        {
            r = *prgb888++;
            g = *prgb888++;
            b = *prgb888++;
            RGB2YUV(b,g,r,y,u,v);
            *py++ = y;
            if ((i & 0x1) && (j & 0x1))
            {
                *pu++ = u;
                *pv++ = v;
            }
        }
    }

    return 0;
}
